function dxdt = dof3PNode(t,x,pp)
global watch_state;
m = PN3_Constant.m;
S_ref = PN3_Constant.S_ref;;g_0 = PN3_Constant.g_0;rho = PN3_Constant.rho;
% 状态量 x = [x y z vx vy vz]
%g = [0;-g_0;0];
g = g_0;
rx = x(1);
ry = x(2);
rz = x(3);
vx = x(4);
vy = x(5);
vz = x(6);
v = sqrt(vx^2 + vy^2 + vz^2);
% persistent inter_num
% if isempty(inter_num)
%     inter_num = 0;
% else 
%     inter_num = inter_num + 1;
% end
% if mod(inter_num,4) == 0
if vy < 0 
% 三维比例导引法：计算视线角速度和过载指令
R_ref = [PN3_Constant.target_x-rx; PN3_Constant.target_y-ry; PN3_Constant.target_z-rz];  % 相对位置矢量
V_ref = [-vx; -vy; -vz];  % 相对速度矢量
R_ref_norm = sqrt((R_ref(1)^2 + R_ref(2)^2 + R_ref(3)^2));
V_close = (R_ref(1)*V_ref(1) + R_ref(2)*V_ref(2) + R_ref(3)*V_ref(3)) / R_ref_norm;  % 相对速度在视线方向的投影
fprintf('------------------------------------\n');
fprintf('R_ref:%.4f,%.4f,%.4f\n',R_ref(1),R_ref(2),R_ref(3));
fprintf('V_ref:%.4f,%.4f,%.4f\n',V_ref(1),V_ref(2),V_ref(3));
%fprintf('R_ref_norm:%.4f\n',R_ref_norm);
fprintf('V_close:%.4f\n',V_close);
% R_ref = [20.0-rx;0.5-ry;-rz];
PN_alpha = atan2(-R_ref(3),R_ref(1));
PN_beta = asin(R_ref(2)/(R_ref(1)^2+R_ref(2)^2+R_ref(3)^2)^0.5);
fprintf('PN_alpha:%.4f,PN_beta:%.4f\n',PN_alpha,PN_beta);
% 视线矢量单位化
%R_unit = R / norm(R);
alpha_dot = (-V_ref(3)*R_ref(1) + V_ref(1)*R_ref(3)) / (R_ref(1)^2 + R_ref(3)^2);
beta_dot = (R_ref_norm*V_ref(2) - V_close*R_ref(2)) / (R_ref(1)^2 + R_ref(3)^2)^0.5; 
fprintf('alpha_dot:%.4f,beta_dot:%.4f\n',alpha_dot,beta_dot);
%比例导引增益
N = 3;
ach = N * alpha_dot * abs(V_close);
acv = N * beta_dot * abs(V_close);

% 视线系投影到制导系
%ac1 = -ach*sin(PN_alpha)-acv*sin(PN_beta)*cos(PN_alpha);
ac1 = 0;
ac2 = ach*cos(PN_alpha)-acv*sin(PN_beta)*sin(PN_alpha);
ac3 = acv*cos(PN_beta);
fprintf('ach:%.4f,acv:%.4f\n',ach,acv);
fprintf('acx:%.4f,acy:%.4f,acz:%.4f\n',ac1,ac3,-ac2);
fprintf('------------------------------------\n');
else
ac1 = 0;
ac2 = 0;
ac3 = 0;
end
% watch_state(1)=ac1;
% watch_state(2)=ac2;
% watch_state(3)=ac3;
% watch_state = [watch_state;ac1,ac2,ac3;];




a_cmd = [ac1;ac3;-ac2];
if norm(a_cmd,2) > 10
    a_cmd = a_cmd./norm(a_cmd).*10.0;
end
if isempty(watch_state)
    watch_state(1,1)=t;
    watch_state(2,1)=ac1;
    watch_state(3,1)=ac3;
    watch_state(4,1)=-ac2;
else
    watch_state = [watch_state,[t;a_cmd]];
end

dxdt = [vx;
        vy;
        vz;
        0%a_cmd(1);
        a_cmd(2)-g;
        a_cmd(3);
        ];
        
end